/*
 * @Description: TL740传感器的数据解析，后续需增加TL740的设置，即数据发送
 * @Author: Dryad
 * @Date: 2019-05-25 15:44:31
 */

#include <rtthread.h>
#include <rtdevice.h>
#include <stm32f4xx_tim.h>
#include <misc.h>
#include "./TL740.h"

static void TL740_Control_Thread(void *parameter);                          /* TL740控制线程 */
static rt_err_t TL740_CallBack(rt_device_t dev, rt_size_t size);            /* 接收回调函数 */
static rt_err_t TL740_Parse_Data(const char *rx_data, const int rx_length); /* TL740解析传感器数据 */
static rt_int32_t BCD2DEC(const char *data);                                /* 将TL740返回的BCD编码数据转换成十进制 */

static TIM_TypeDef *const tim = TIM6;         /* 超时检测定时器为TIM6 */
static void Set_Time(unsigned long baudrate); /* 设置数据帧检测定时器 */

static struct rt_thread tl740_thread; /* TL740线程句柄 */
ALIGN(RT_ALIGN_SIZE)                  /* 线程栈4字节对齐 */
static char tl740_thread_stack[512];  /* 线程栈起始地址 */
static rt_device_t tl740_dev = RT_NULL;

static struct rt_semaphore dev_rx_sem; /* 串口接收到数据信号量 */
static char tl740_rx_data[64] = {0};   /* TL740接收数据 */
static int tl740_rx_length = 0;        /* TL740接收数据长度 */

static float tl740_z_rate = 0.0f;          /* Z轴角速率(°/s) */
static float tl740_z_heading = 0.0f;       /* Z轴方位角，-180°~+180° */
static float tl740_z_heading_baise = 0.0f; /* Z轴方位角基数据 */

static struct rt_semaphore tl740_sem; /* 接收到正确数据信号量 */

void TL740_Thread_Init(void)
{
    rt_err_t rt_result = RT_EOK;
    tl740_dev = rt_device_find(TL740_DEVICE_NAME);

    if (tl740_dev == RT_NULL)
    {
        rt_kprintf("TL740 Uart Error\n");
        return;
    }

    /* 设置波特率 */
    struct serial_configure config = RT_SERIAL_CONFIG_DEFAULT;
    config.bufsz = 128;

    rt_device_control(tl740_dev, RT_DEVICE_CTRL_CONFIG, &config);

    rt_sem_init(&dev_rx_sem, "tl740 rx sem", 0, RT_IPC_FLAG_FIFO);  /*指示串口接收到了数据*/
    rt_sem_init(&tl740_sem, "tl740 data sem", 0, RT_IPC_FLAG_FIFO); /*指示TL740接收到正确数据*/
    Set_Time(115200);
    rt_device_open(tl740_dev, RT_DEVICE_OFLAG_RDONLY | RT_DEVICE_FLAG_INT_RX); /*中断接收*/

    rt_device_set_rx_indicate(tl740_dev, TL740_CallBack);

    rt_result = rt_thread_init(&tl740_thread, "tl740 thread", TL740_Control_Thread, RT_NULL, tl740_thread_stack, sizeof(tl740_thread_stack), 4, 2);
    if (rt_result == RT_EOK)
    {
        rt_thread_startup(&tl740_thread); /* 启动线程 */
    }
    else
    {
        rt_kprintf("tl740 thread error\n");
    }
}

rt_err_t TL740_Return_Data(rt_int32_t wait_time, float *z_rate, float *z_heading)
{
    rt_err_t rt_result = RT_EOK;
    rt_result = rt_sem_take(&tl740_sem, wait_time);
    *z_rate = tl740_z_rate;
    *z_heading = tl740_z_heading;
    return rt_result;
}

void TL740_Control_Thread(void *parameter)
{
    rt_err_t rt_result = RT_EOK;
    rt_size_t read_size = 0;
    rt_err_t parse_result = RT_EOK;

    while (1)
    {
        rt_result = rt_sem_take(&dev_rx_sem, RT_WAITING_FOREVER); /* 等待接收数据 */
        if (rt_result != RT_EOK)
        {
            continue;
        }
        /* 长度限幅 */
        if (tl740_rx_length > sizeof(tl740_rx_data))
        {
            tl740_rx_length = sizeof(tl740_rx_data);
        }
        read_size = rt_device_read(tl740_dev, 0, tl740_rx_data, tl740_rx_length); /* 读取数据 */
        parse_result = TL740_Parse_Data(tl740_rx_data, read_size);                /* 解析应答数据 */

        /* 解析成功 */
        if (RT_EOK == parse_result)
        {
            rt_sem_release(&tl740_sem);
        }
    }
}

rt_err_t TL740_CallBack(rt_device_t dev, rt_size_t size)
{
    tl740_rx_length = size;  /* 保存接收数据长度 */
    tim->CNT = 0;            /* 计数器清0 */
    tim->CR1 |= TIM_CR1_CEN; /* 使能定时器 */
    return RT_EOK;
}

rt_err_t TL740_Parse_Data(const char *rx_data, const int rx_length)
{
    for (int j = 0; j + 14 <= rx_length; j++)
    {
        if (rx_data[j] != 0x68)
        {
            continue; //寻找帧头
        }

        char bc = 0;

        for (int i = 1; i < 13; i++)
        {
            bc += rx_data[i + j];
        }

        if (bc == rx_data[j + 13])
        {
            rt_int32_t z_rate_temp = 0, z_head_temp = 0;
            z_rate_temp = BCD2DEC((&rx_data[j + 4]));
            z_head_temp = BCD2DEC((&rx_data[j + 10]));

            tl740_z_rate = z_rate_temp / 100.0f;
            tl740_z_heading = z_head_temp / 100.0f + tl740_z_heading_baise;
            return RT_EOK;
        }
    }
    return -RT_ERROR;
}

rt_int32_t BCD2DEC(const char *data)
{
    rt_int32_t temp = 0;
    for (int i = 0; i < 3; i++)
    {
        temp *= 100;
        temp += ((*data & 0xF0) >> 4) * 10 + (*data & 0x0F);
        ++data;
    }
    if (temp >= 100000)
    {
        temp = 100000 - temp;
    }
    return temp;
}

/**
 * @description: 清除陀螺仪内部角度数据，并记录当前角度
 * @param {type} 
 * @return: 
 */
void Clear_Accumulated_Error(float current_z_heading)
{
    tl740_z_heading_baise = current_z_heading;
    rt_device_write(tl740_dev, 0, "\x68\x04\x00\x28\x2c", 5);
}

/* 设置数据帧检测定时器 */
void Set_Time(unsigned long baudrate)
{
    rt_uint16_t usTimer_50us = 0;
    /* The timer reload value for a character is given by:
		*
		* ChTimeValue = Ticks_per_1s / ( Baudrate / 10 )
		*             = 10 * Ticks_per_1s / Baudrate
		*             = 200000 / Baudrate
		*/
    usTimer_50us = (2 * 200000UL / baudrate); /* 超时时间为2个字符 */

    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
    NVIC_InitTypeDef NVIC_InitStructure;

    /* 设置定时器时基20KHz */
    TIM_TimeBaseStructure.TIM_Period = usTimer_50us - 1;
    TIM_TimeBaseStructure.TIM_Prescaler = 4200 - 1;
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure);

    tim->CR1 &= ~(1 << 1);    //清空UDIS,使能更新中断UEV
    tim->CR1 |= (1 << 2);     //置位URS,只有计数器上溢会生成UEV（更新中断）
    tim->SR = ~TIM_IT_Update; //清零更新中断标志

    tim->DIER |= TIM_IT_Update; //使能更新中断

    NVIC_InitStructure.NVIC_IRQChannel = TIM6_DAC_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; //抢占优先级
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;        //响应优先级
    NVIC_Init(&NVIC_InitStructure);
}

void TIM6_DAC_IRQHandler(void)
{
    rt_interrupt_enter();
    if (tim->SR & TIM_IT_Update) //更新中断
    {
        /* 定时器更新中断 */
        tim->SR = ~TIM_IT_Update;
        tim->CR1 &= ~TIM_CR1_CEN;    /* 关闭定时器 */
        rt_sem_release(&dev_rx_sem); /* 释放信号量，通知处理线程 */
    }
    rt_interrupt_leave();
}
